package org.reborn.engine.model.mesh

import scala.math
import scala.collection.mutable.ListBuffer
import scala.collection.mutable.PriorityQueue

import _root_.org.reborn.math.util.MathUtils
import _root_.org.reborn.math._
import _root_.org.reborn.engine.enums.ArrayType._
import _root_.org.reborn.engine.enums.FaceMode._
import _root_.org.reborn.utils.ListUtil

import scala.collection.mutable.ArrayBuffer
import scala.collection.mutable.HashMap

import java.nio.FloatBuffer
import java.nio.IntBuffer

object MeshConnectivityUtil {
	
  type EqualsFunc = (HashMap[ArrayType,VectorF],HashMap[ArrayType,VectorF]) => Boolean
  type EdgeCostFunc = (ConnectedEdge) => Float
  type MidpointFunc = (ConnectedEdge) => HashMap[ArrayType,VectorF]
  type ConditionFunc = (Int,Float) => Boolean

  /**
   * Create a mutable mesh from a MeshConnectivity object.
   */
  def MutableMesh(meshCon:MeshConnectivity):MutableMesh = {
	require(meshCon.vertices.size > 0, "Mesh connectivity must contain at least 1 vertex.")
	require(meshCon.vertices(0).dataMap.contains(Vertex), "Mesh connectivity must contain vertex data.")
	
	val aTypes = meshCon.vertices(0).dataMap.keySet
    val faces = meshCon.faces
    val numFaces = faces.size
    val verts = meshCon.vertices
    val numVerts = verts.size
    
    val mesh = MeshUtil.MutableMesh(Triangle,faces.size)
    
    for(aType <- aTypes){
      val partsPerVert = verts(0).dataMap(aType).length
      val vertArray = new Array[Float](numVerts * partsPerVert)

      for(i <- 0 until numVerts){
        val vertData = verts(i).dataMap(aType)

        for(j <- 0 until partsPerVert){
	      vertArray(i*partsPerVert + j) = vertData(j)
	    }
      }
      mesh.replaceVertData(aType, partsPerVert, vertArray)
	}
    
	val vertIndexMap = calcVertexIndexMap(meshCon)
	
	val faceArray = new Array[Int](numFaces * 3)
	for(i <- 0 until numFaces; j <- 0 until 3){
	  val face = faces(i)
	  val vertex = face.getVertex(j)
	  val index = vertIndexMap(vertex)
	  faceArray(i*3 + j) = index 
	}
	
	mesh.replaceFaceData(faceArray)
    mesh
  }
  
  /**
   * Finds the connectivity of a mesh in terms of vertices, faces, and edges.
   * @param mesh The mesh to get connectivity data for
   * @param withVertData Specifies whether or not the connectivity will include vertex data from the mesh.
   */
  def calcMeshConnectivity(mesh:Mesh,withVertData:Boolean):MeshConnectivity = {
	  val types = if(withVertData){ mesh.getAvailableArrayTypes } else { List() }
	
	  calcMeshConnectivity(mesh,types)
  }
  
  /**
   * Finds the connectivity of a mesh in terms of vertices, faces, and edges.
   * @param mesh The mesh to get connectivity data for
   * @param vertDataTypes Specifies the types of vertex data to also include in the connectivity data.
   */
  def calcMeshConnectivity(mesh:Mesh,vertDataTypes:Iterable[ArrayType]):MeshConnectivity = {
	require(mesh.getFaceMode == Triangle)
	  
	val verts1 = System.nanoTime
	//Create vertices for each vertex in the mesh.  For all array types specified, 
	//add the data to the info map
	var i=0;
	var verts=mesh.verts
	var vertList:ArrayBuffer[ConnectedVertex] = new ArrayBuffer(verts)
	while(i < verts){
	  val vert = new ConnectedVertex()
	  vertList += vert
	  for(aType <- vertDataTypes){
	 	var vertDataList:List[Float] = Nil
	 	val vertData = mesh(aType)
	 	val ppVertex = vertData.partsPerVertex
	 	val vertDataArray = vertData.getDataArray
	 	for(j <- 0 until ppVertex){
	 	  vertDataList = vertDataList ::: List(vertDataArray(ppVertex * i + j))
	 	}
	 	vert.dataMap += ((aType,VectorF(vertDataList)))
	  }
	  i+=1
	}
	val verts2 = System.nanoTime
	println("Create Vertices: "+((verts2 - verts1) / 1000000.0))
	
	var edges1 = 0.0;
	
	//Connect all the vertices created in the previous step to the right faces and edges
	i=0
	var faces=mesh.faces
	val faceData = mesh.getFacesArray
	var faceList:ArrayBuffer[ConnectedFace] = new ArrayBuffer(faces)
	var edgeList:ArrayBuffer[ConnectedEdge] = new ArrayBuffer(faces * 3)
	val vertsPerFace = mesh.verticesPerFace
	while(i < faces){
	  val face = new ConnectedFace();
	  faceList += face
	  val faceOffset = i * vertsPerFace;
	  var j = 0;
	   
	  while(j < vertsPerFace){
	 	val curVert = vertList(faceData(faceOffset + j))
	 	val nextVert = vertList(faceData(faceOffset + MathUtils.wrappedIndex(j+1,vertsPerFace)))
	 	face.addVertex(curVert);
	 	curVert.addFace(face);

	 	val edges2 = System.nanoTime
	 	val edge = new ConnectedEdge(curVert,nextVert);
	 	val existingEdgeInd = 
	 	if(curVert.getEdgeIndex(edge) < 0 && nextVert.getEdgeIndex(edge) < 0){
	 	  curVert.addEdge(edge);
	 	  nextVert.addEdge(edge);
	 	  edgeList += edge
	 	}
	 	val edges3 = System.nanoTime
	    edges1 += edges3 - edges2;
	 	j+=1
	  }
	  
	  i+=1
	}
	val faces1 = System.nanoTime
	println("Create Faces and Edges: "+((faces1 - verts2) / 1000000.0))
	println("Create Edges: "+((edges1) / 1000000.0))

	val meshCon = new MeshConnectivity()
	meshCon.vertices = vertList;
	meshCon.faces = faceList;
	meshCon.edges = edgeList;
	meshCon
  }
  
  /**
   * Calculates normals on a per-face basis. 
   * @param meshCon The mesh connectivity to modify
   * @param angleDegrees An angle in degrees that is used to determine if a vertex that isn't
   * explicitly shared by 2 faces should take the average of the face normals.
   * @param isEquivalent a Function used to determine if 2 vertices should be considered equal
   * @return the modified MeshConnectivity object
   */
  def calculateNormals(meshCon:MeshConnectivity,angleDegrees:Float,equivVerts:Map[ConnectedVertex,Set[ConnectedVertex]]):MeshConnectivity = {
	import org.reborn.utils.TimeUtil
	
	val beforeEquivVerts = System.nanoTime
	val afterEquivVerts = System.nanoTime
	println("Calc equivalent vertices: "+((afterEquivVerts - beforeEquivVerts) / 1000000.0))
	
	
	val beforeCalcNorms = System.nanoTime
	
	val vertIndexMap = calcVertexIndexMap(meshCon)
	
	//First, calculate the normals for each face
	var faceNormals:Map[ConnectedFace,VectorF] = Map()
	val faceList = meshCon.faces
	val vertList = meshCon.vertices
	for(face <- faceList){
	  val v1 = face.getVertex(0)
	  val v2 = face.getVertex(1)
	  val v3 = face.getVertex(2)
	  val vec1 = v1.dataMap(Vertex) - v2.dataMap(Vertex)
	  val vec2 = v2.dataMap(Vertex) - v3.dataMap(Vertex)
	  faceNormals += ((face,(vec1 cross vec2).normalize))
	}
	
	val afterCalcNorms = System.nanoTime
	println("Calc face normals: "+((afterCalcNorms - beforeCalcNorms) / 1000000.0))
	
	
	val beforeVertNorms = System.nanoTime
	//Loop through each vertex, and get the average of the normals at each 
	//face that it is directly connected to. Average in the faces that it
	//is not directly connected to if and only if the angle between the current
	//average and the face is less than the user-specified angle
	var i=0;
	while(i<vertList.size){
	  val vert = vertList(i)
	  var connectedFaces = vert.getFaces
	  var faces = 0
	  var sum = VectorF(List(0.0f,0.0f,0.0f))
	  for(face <- connectedFaces){
	 	sum = sum + faceNormals(face)
	 	faces += 1
	  }
	  val avg = sum / faces.asInstanceOf[Float]
	  
	  //Create a set of all the faces that are connected to the equivalent
	  //vertices. But remove the faces that have already been calculated in
	  //the average.
	  val equivList = equivVerts.get(vert).getOrElse(Set())
	  var touchingFaces:Set[ConnectedFace] = Set()
	  vert.getFaces.foreach(elm => touchingFaces = touchingFaces + elm)
	  touchingFaces = touchingFaces -- connectedFaces	//remove all the faces that 
	  
	  var equivFaces = 0;
	  var equivSum = VectorF(List(0.0f,0.0f,0.0f))
	  //Average all the face normals that are less than the specified angle
	  for(face <- touchingFaces){
	    val faceNorm = faceNormals(face)
	    if(math.toDegrees(math.acos(faceNorm dot avg)) <= angleDegrees){
	      equivFaces += 1
	      equivSum = equivSum + faceNorm
	    }
	  }
	  
	  sum = ((sum * faces) + (equivSum * equivFaces)).normalize
	  vert.dataMap += (Normal -> sum)
	  
	  i+=1
	}

	val afterVertNorms = System.nanoTime
	println("Calc vertex normals: "+((afterVertNorms - beforeVertNorms) / 1000000.0))
	
	meshCon
  }
  
  private def calcVertexIndexMap(meshCon:MeshConnectivity):Map[ConnectedVertex,Int] = {
	var map:Map[ConnectedVertex,Int] = Map()
	val verts = meshCon.vertices
	var i=0
	for(vert <- verts){
	  map += (vert -> i)
	  i+=1
	}
	map
  }
  
  private def calculateEquivVertices(meshCon:MeshConnectivity,isEquivalent:EqualsFunc):Map[ConnectedVertex,Set[ConnectedVertex]] = {
	var map:Map[ConnectedVertex,Set[ConnectedVertex]] = Map()
	
	val vertList = meshCon.vertices
	val length = vertList.size
	var i=0
	while(i < length){
	  val curVert = vertList(i)
	  val v1 = curVert.dataMap
	   
	  var j = i+1
	  while(j < length){
        val testVert = vertList(j)
	    val v2 = testVert.dataMap
	 	
	    if(isEquivalent(v1,v2)){
	      var setV1 = map.get(curVert).getOrElse(Set())
	 	  setV1 += testVert
	 	  map+=(curVert -> setV1)
	 	 	
	 	  var setV2 = map.get(testVert).getOrElse(Set())
	 	  setV2 += curVert
	 	  map+=(testVert -> setV2)  
	 	}
	 	
	 	j+=1
	  }
	  
	  i+=1
	}
	
	map
  }
  
  /**
   * 
   * @param meshCon
   * @return
   */
  def reduceMesh(meshCon:MeshConnectivity,
		  edgeCostFunc:EdgeCostFunc,
		  midpointFunc:MidpointFunc,
		  conditionFunc:ConditionFunc):MeshConnectivity = {

    val beforeCreateQueue = System.nanoTime
	
    //Calculate the cost of removing each edge and put it in the queue
    implicit object EdgePriorityOrdering extends Ordering[ConnectedEdge] {
      def compare(x:ConnectedEdge,y:ConnectedEdge):Int = {
        - x.weight.compare(y.weight)
      }
    }
    var edgePQ = new PriorityQueue[ConnectedEdge]
    for(edge <- meshCon.edges){
      edge.weight = edgeCostFunc(edge)
      edgePQ += (edge)
    }
    val afterCreateQueue = System.nanoTime
	println("Calc initial edge weights: "+((afterCreateQueue - beforeCreateQueue) / 1000000.0))
    
    val badMesh = meshCon.verifyConnectivity()
    if(badMesh.isDefined)
    	println(badMesh.get)
    
    //Remove edges while we satisfy the conditional function
    var curEdge = edgePQ.dequeue
    var i=0;
    while(conditionFunc(i,curEdge.weight) && edgePQ.size > 3){
      val beforeRemoveEdge = System.nanoTime
    	
      //Create a new vertex at the place where the edge is being removed
      val newVert = new ConnectedVertex()
      newVert.dataMap ++= midpointFunc(curEdge)
      
      //Get the list of vertices that we are merging together
      val mergedVerts = curEdge.getVertices
      
      //Get the list of faces that we will be removing
      val removedFaces = curEdge.connectedFaces
      
      //Find the vertices that are attached to each removed face that 
      //are not part of the removed edge
      var faceWithOppositeVertList:List[(ConnectedFace,ConnectedVertex)] = Nil
      for(face <- removedFaces; vert <- face.getVertices){
    	  val faceAndVert = (face,vert)
    	  if(!mergedVerts.contains(vert) && !faceWithOppositeVertList.contains(faceAndVert))
    	 	  faceWithOppositeVertList = faceAndVert :: faceWithOppositeVertList
      }
      val oppositeVerts = faceWithOppositeVertList.map((faceAndVert)=> faceAndVert._2)
      
      //Replace the old vertex in each face and edge with the new one
      var facesAddedToNewVert:List[ConnectedFace] = Nil
      var edgesAddedToNewVert:List[ConnectedEdge] = Nil
      var removedEdges:List[ConnectedEdge] = Nil
      for(vert <- mergedVerts){
    	  for(face <- vert.getFaces){
    	 	  if(!removedFaces.contains(face) && !facesAddedToNewVert.contains(face)){
	    	 	  face.replaceVertex(vert, newVert)
	    	 	  facesAddedToNewVert = face :: facesAddedToNewVert
    	 	  }
    	  }
    	  for(edge <- vert.getEdges){
    	 	  if(!(edge eq curEdge)){
    	 	 	  if(edge.containsVertex(vert))
    	 	 		  edge.replaceVertex(vert, newVert)
//    	 	 	  if(edge.containsVertex(vert) || !edge.containsVertex(newVert))
//    	 	 	 	  println("WTF!");
    	 	 	  if(!edgesAddedToNewVert.contains(edge)){
    	 	 		  edgesAddedToNewVert = edge :: edgesAddedToNewVert
    	 	 	  }
    	 	 	  else{
    	 	 	 	  removedEdges = edge :: removedEdges
    	 	 	  }
    	 	  }
    	  }
      }
      
      //Add the edge or face to the new vertex
      newVert.getFaces ++= facesAddedToNewVert
      newVert.getEdges ++= edgesAddedToNewVert
      
      //Remove any duplicate edges that are connected the oppositeVerts
      //Also, remove the face that was touching the vertex
      for((face,vert) <- faceWithOppositeVertList){
    	  vert.removeFace(face)
    	  for(edge <- removedEdges){
    	 	  if(vert.containsEdge(edge))
    	 	 	  vert.removeEdge(edge)
    	  }
      }
      
      //Remove all affected edges from the priority queue
      val allEdges = edgesAddedToNewVert ::: removedEdges
      edgePQ = edgePQ.filterNot((edge) => allEdges.exists(_ eq edge))
      
      //Remove the merged and current edges from the mesh
      //Add the new edges to the mesh
      val allModifiedEdges = curEdge :: removedEdges ::: edgesAddedToNewVert
      meshCon.edges = meshCon.edges.filterNot((edge) => allModifiedEdges.exists(_ eq edge))
      meshCon.edges ++= edgesAddedToNewVert
      
      //Remove the deleted faces from the mesh
      meshCon.faces --= removedFaces
      
      //Remove the merged vertices from the mesh
      //Add the new vertex to the mesh
      meshCon.vertices --= mergedVerts
      meshCon.vertices += newVert
      
      //Finally, recalculate the changed edges' weights and add them back to the priority queue
      for(edge <- edgesAddedToNewVert){
    	  edge.weight = edgeCostFunc(edge)
    	  edgePQ += (edge)
      }
      
//      verifyMeshCon();
      
      val afterRemoveEdge = System.nanoTime
      println("Remove Edge: "+((afterRemoveEdge - beforeRemoveEdge) / 1000000.0))
      println("Faces: " + meshCon.faces.size + ", Verts: " + meshCon.vertices.size + ", Edges: " + meshCon.edges.size +", PQ: " + edgePQ.size)
      
      //Get the next edge in the queue
      curEdge = edgePQ.dequeue
      i+=1
    }
    
    val afterRemoveEdges = System.nanoTime
    println("Remove all Edges: "+((afterRemoveEdges - afterCreateQueue) / 1000000.0))

    meshCon
  }
  
  /**
   * 
   * @param meshCon
   * @return
   */
  def expandVertices(meshCon:MeshConnectivity):MeshConnectivity = {
	  val faceList = meshCon.faces
	  var newVertList:ArrayBuffer[ConnectedVertex] = new ArrayBuffer(faceList.size * 3);
	  var newEdgeList:ArrayBuffer[ConnectedEdge] = new ArrayBuffer(faceList.size * 3);
	  
	  for(face <- faceList){
	 	val vertsPerFace = face.getVertices.size
	 	var i = 0;
	 	//Create new vertices for each face
	 	while(i < vertsPerFace){
	 	  val vert = new ConnectedVertex()
	 	  val oldVert = face.getVertex(i)
	 	  vert.addFace(face)
	 	  face.replaceVertex(oldVert, vert);
	 	  newVertList += vert
	 	  
	 	  val oldVertIndex = meshCon.vertices.indexOf(oldVert)
	 	  for(aType <- oldVert.dataMap.keySet){
	 	 	vert.dataMap += (aType -> oldVert.dataMap(aType))
	 	  }
	 	  
	 	  i+=1
	 	}
	 	i=0;
	 	//Create new edges with the new vertices
	 	while(i < vertsPerFace){
	 	  val next = MathUtils.wrappedIndex(i+1, face.getVertices.size) 
	 	  val vert = face.getVertex(i)
	 	  val nextVert = face.getVertex(next)
	 	  val edge = new ConnectedEdge(vert,nextVert)
	 	  vert.addEdge(edge)
	 	  nextVert.addEdge(edge)
	 	  newEdgeList += edge
	 	  i+=1
	 	}
	  }
	  
	  meshCon.vertices = newVertList
	  meshCon.edges = newEdgeList
	  meshCon
  }
  
  /**
   * Removes duplicate vertices in a MeshConnectivity object based on a user-defined set
   * of limits for each ArrayType.
   * @param meshCon The mesh connectivity to modify
   * @param isDuplicate is a Function used to determine if 2 vertices should be considered equal
   * @return the modified MeshConnectivity object
   */
  def removeDuplicateVertices(meshCon:MeshConnectivity,isDuplicate:EqualsFunc):MeshConnectivity = {
	  var equivVertsMap = calculateEquivVertices(meshCon,isDuplicate)
	  
	  var i = 0
	  while(i < meshCon.vertices.size){
	 	val vert = meshCon.vertices(i)
	 	
	    var equivVerts = equivVertsMap.get(vert).getOrElse(Set())
	 	var sameCount = 0
	 	for(remVert <- equivVerts){
	 	  val remVertIndex = meshCon.vertices.indexOf(remVert)
	 	  if(remVertIndex >= 0){
	 	 	sameCount += 1
	 	 	
	 	 	meshCon.vertices.remove(remVertIndex)
	 	 	equivVertsMap += (vert -> (equivVertsMap(vert) - remVert))
	 	 	 	
     	 	//Replace the vertex in any faces that were connected
     	 	for(face <- remVert.getFaces){
     	 	  vert.addFace(face)
     	 	  face.replaceVertex(remVert,vert)
     	 	}
     	 	
     	 	//Replace the vertex in any edges that were connected
     	 	for(edge <- remVert.getEdges){
     	 	  //replace the duplicated vertex in the edge with the original vertex
     	      edge.replaceVertex(remVert,vert)
     	      
     	      //If the updated edge already exists in the original vertex,
     	 	  //then remove the edge from the list of edges (deleting the edge)
     	 	  if(vert.containsEdge(edge)){
     	 	 	vert.removeEdgeReference(edge)
     	 	 	edge.otherVertex(vert).removeEdge(edge)
     	 	 	val edgeInd = meshCon.edges.indexOf(edge)
     	 	 	meshCon.edges.remove(edgeInd)
     	 	  }
     	      //Otherwise, add the edge to the original vertex
     	 	  else{
     	        vert.addEdge(edge)
     	 	  }
     	 	}
	 	  }
	 	}
	 	
	 	i+= 1
	  }
	  
	  meshCon
  }
  
  def vertexCoordsAreSame(v1:HashMap[ArrayType,VectorF],v2:HashMap[ArrayType,VectorF],dist:Float):Boolean = {
	arrayTypeHasSameCoordinates(v1,v2,Vertex,dist)
  }
  def colorsAreSame(v1:HashMap[ArrayType,VectorF],v2:HashMap[ArrayType,VectorF],dist:Float):Boolean = {
	arrayTypeHasSameCoordinates(v1,v2,Color,dist)
  }
  def textureCoordsAreSame(v1:HashMap[ArrayType,VectorF],v2:HashMap[ArrayType,VectorF],textureUnit:Int,dist:Float):Boolean = {
	textureUnit match{
		case 1 => arrayTypeHasSameCoordinates(v1,v2,TexCoord1,dist)
		case 2 => arrayTypeHasSameCoordinates(v1,v2,TexCoord2,dist)
		case 3 => arrayTypeHasSameCoordinates(v1,v2,TexCoord3,dist)
		case 4 => arrayTypeHasSameCoordinates(v1,v2,TexCoord4,dist)
		case _ => throw new IllegalArgumentException("Only texture unit 1-4 are allowed. Found:"+textureUnit)
	}
  }
  def normalsAreSame(v1:HashMap[ArrayType,VectorF],v2:HashMap[ArrayType,VectorF],angleDegrees:Float):Boolean = {
	arrayTypeVectorIsWithinSpecifiedAngle(v1,v2,Normal,angleDegrees)
  }
  
  def arrayTypeHasSameCoordinates(v1:HashMap[ArrayType,VectorF],v2:HashMap[ArrayType,VectorF],aType:ArrayType,dist:Float):Boolean = {
	val vert1 = v1(aType)
	val vert2 = v2(aType)
	(vert2 - vert1).magnitude <= dist
  }
  
  def arrayTypeVectorIsWithinSpecifiedAngle(v1:HashMap[ArrayType,VectorF],v2:HashMap[ArrayType,VectorF],aType:ArrayType,angleDegrees:Float):Boolean = {
	val vert1 = v1(aType).normalize
	val vert2 = v2(aType).normalize
	val angleRads = math.acos(vert1 dot vert2)
	math.toDegrees(angleRads) <= angleDegrees
  }
}

